import math

MIN_ACCURACY = pow(0.05, 2)
STEP_SIZE = 0.02

class Circle(object):
    def __init__(self, location, speed, radius):
        self.p = location
        self.v = speed
        self.r = radius

c1 = Circle((5,7), (0,-10), 1)
c2 = Circle((0,0), (10,0), 1)
#c1 = Circle((-10,10), (10,-10), 1)
#c2 = Circle((0,0), (-10,10), 1)

def performCollision(c1, c2, p1, p2):
    #solve for X first
    deltaX1 = p2[0] - p1[0]
    deltaY1 = p2[1] - p1[1]
    deltaX2 = p1[0] - p2[0]
    deltaY2 = p1[1] - p2[1]
    m1 = 1
    m2 = 1
    gammav = math.atan2(c1.v[1], c1.v[0])
    alpha = gammav - math.atan2(deltaY1, deltaX1)
    a = math.tan(gammav + alpha)
    dv2xf = 2 * (c1.v[0] - c2.v[0] + a * (c1.v[1] - c2.v[1])) / ((1 + pow(a, 2)) * (1 + m2 / m1))
    v1xf = c1.v[0] - m2 / m1 * dv2xf
    v1yf = c1.v[1] - a * m2 / m1 * dv2xf
    v2xf = c2.v[0] + dv2xf
    v2yf = c2.v[1] + a * dv2xf
    return (v1xf, v1yf), (v2xf, v2yf)

def checkCollision(p1, p2, fixedDist):
    dist = pow(p1[0]-p2[0], 2) + pow(p1[1]-p2[1], 2)
    if dist < fixedDist:
        return True
    else:
        return False

def binSearch(o1, o2, p1, p2, d):
    midPoint1 = ((o1[0] + p1[0])/2, (o1[1] + p1[1])/2)
    midPoint2 = ((o2[0] + p2[0])/2, (o2[1] + p2[1])/2)
    midPointCollides = checkCollision(midPoint1, midPoint2, d)
    if midPointCollides:
        #check for condition to stop recursion
        if pow(midPoint1[0] - o1[0], 2) + pow(midPoint1[1] - o1[1], 2) < MIN_ACCURACY and pow(midPoint2[0] - o2[0], 2) + pow(midPoint2[1] - o2[1], 2) < MIN_ACCURACY:
            return ((midPoint1[0]+o1[0])/2, (midPoint1[1]+o1[1])/2), ((midPoint2[0]+o2[0])/2, (midPoint2[1]+o2[1])/2)
        else:
            #else call binSearch on the questionable area.
            return binSearch(o1, o2, midPoint1, midPoint2, d)
    else:
        #check for condition to stop recursion
        if pow(midPoint1[0] - o1[0], 2) + pow(midPoint1[1] - o1[1], 2) < MIN_ACCURACY and pow(midPoint2[0] - o2[0], 2) + pow(midPoint2[1] - o2[1], 2) < MIN_ACCURACY:
            return ((midPoint1[0]+p1[0])/2, (midPoint1[1]+p1[1])/2), ((midPoint2[0]+p2[0])/2, (midPoint2[1]+p2[1])/2)
        else:
            #else call binSearch on the questionable area.
            return binSearch(midPoint1, midPoint2, p1, p2, d)

#Find where two lines (velocity vectors) intersect.
#Then find first point at which the circle radii can meet.
#Then find an equation to bring in time as a constraint, and find out if the circles will ever be near enough to collide.

#Actually, step search is good enough.
#linear step to intersect, followed by binary search until satisfied.
#add 1 to pixel size of objects, or to check distance.
def collide(c1, c2):
    #Find step size. About 15 degrees on the smaller circle.
    #Now step size is 2% of the full velocity vector. This should be good enough.
    #Changed my mind. It's a global constant now.
    
    #Find the minimum distance between objects required to cause a collision.
    #One extra pixel will take care of some misses. Might want to base the additional constant on arc step instead though.
    minDist = c1.r + c2.r
    #Square it to avoid square rooting later
    fixedDist = pow(minDist,2)
    #Set up positions
    old1 = (0, 0)
    old2 = (0, 0)
    p1 = c1.p
    p2 = c2.p
    while(True):
        #Step
        old1 = p1
        old2 = p2
        p1 = (p1[0] + STEP_SIZE * c1.v[0], p1[1] + STEP_SIZE * c1.v[1])
        p2 = (p2[0] + STEP_SIZE * c2.v[0], p2[1] + STEP_SIZE * c2.v[1])
        #Check distance from p1 to p2
        dist = pow(p1[0] - p2[0], 2) + pow(p1[1] - p2[1], 2)
        if dist <= fixedDist:
            break
        elif p1[0] > (c1.p[0] + c1.v[0]):
            print "No collisions detected"
            exit(0)
        #print "Collided with c1 at: (%0.2f, %0.2f) and c2 at: (%0.2f, %0.2f)"%(p1[0], p1[1], p2[0], p2[1])
    
    p1, p2 = binSearch(old1, old2, p1, p2, fixedDist)
    print "Collided with c1 at:"+str(p1)+" and c2 at:"+str(p2)
    vf1, vf2 = performCollision(c1, c2, p1, p2)
    print "final veolcity for c1 was:"+str(vf1)+" and c2:"+str(vf2)

#Can also find intersection of lines tangent to the edges of the circles.